#include "Point.h"

#include <assert.h>

#include <cmath>

//#include "../../Common/include/Constant.h"

using namespace SG::Algebra;

// using SG::DataStructure::Common::TOLERANCE;
// using SG::Algebra::Point;

namespace SG
{
    namespace Algebra
    {
        bool operator== (const Point& a, const Point& b)
        {
            if (fabs (a.m_x - b.m_x) > TOLERANCE || fabs (a.m_y - b.m_y) > TOLERANCE || fabs (a.m_z - b.m_z) > TOLERANCE)
            {
                return false;
            }

            return true;
        }

        Vector3D Distance (const Point& a, const Point& b)
        {
            Vector3D v;
            v.m_x = a.m_x - b.m_x;
            v.m_y = a.m_y - b.m_y;
            v.m_z = a.m_z - b.m_z;
            return v;
        }

        Real_t length (const Point& a, const Point& b)
        {
            return sqrt ((a.m_x - b.m_x) * (a.m_x - b.m_x) + (a.m_y - b.m_y) * (a.m_y - b.m_y) + (a.m_z - b.m_z) * (a.m_z - b.m_z));
        }
    }  // namespace SG::Algebra
}


void Point::Transform (const Matrixd& T, const Point& origin, const Point& p0, _OUT Point& pNew)
{
    // 检验转换矩阵维度
    assert (3 == T.GetColSize () && 3 == T.GetRowSize ());

    auto r = Distance (p0, origin);

    pNew.m_x = T (0, 0) * r.m_x + T (0, 1) * r.m_y + T (0, 2) * r.m_z;
    pNew.m_y = T (1, 0) * r.m_x + T (1, 1) * r.m_y + T (1, 2) * r.m_z;
    pNew.m_z = T (2, 0) * r.m_x + T (2, 1) * r.m_y + T (2, 2) * r.m_z;
}

